#pragma once
#include "rclcpp/rclcpp.hpp"
#include <memory>
#include <future>
#include <hardware_interface.h>
#include <resource_manager/controller_base.h>
#include <resource_manager/joint_handle.h>
#include <std_msgs/msg/float32.hpp>
#include <realtime_tools/realtime_publisher.h>
#include <sensor_msgs/msg/joint_state.hpp>
#include <agv_msgs/msg/ca_nopen_node_status.hpp>
#include <agv_srvs/srv/controller_enable.hpp>

namespace resource_manager
{

class ControllerManager
{
public:

    enum class ControllerState
    {
        CONSTRUCTED = 0,
        INITIALIZING,
        RUNNING,
        HALT,
    };


    ControllerManager(rclcpp::Node &nh, ros_canopen_driver::HardwareInterface* hi);
    ~ControllerManager();

    bool init();
    bool init_joint_states_publisher();
    void publish_joint_states(const rclcpp::Time& time);
    void update(const rclcpp::Time& time, const rclcpp::Duration& period);
    void stop();
    void node_status_callback(const agv_msgs::msg::CANopenNodeStatus::ConstSharedPtr& msg);
    bool controller_node_ready(ControllerBase* controller);
    bool init_servo(ControllerBase* controller);
    bool halt_servo(ControllerBase* controller);
    bool recover_servo(ControllerBase* controller);
    bool controller_enable_callback(agv_srvs::srv::ControllerEnable::Request &req, agv_srvs::srv::ControllerEnable::Response &resp);

private:
    rclcpp::Node                         nh_;
    std::vector<ControllerBase*>            controllers_;
    ControllerState                         state_;
    ros_canopen_driver::HardwareInterface*  hi_;
    JointHandleCollector*                   jhc_;
    int                                     num_joints_;
    std::shared_ptr<realtime_tools::RealtimePublisher<sensor_msgs::msg::JointState> >  realtime_pub_;
    rclcpp::Time                               last_publish_time_;
    double                                  publish_rate_;
    ros::Subscriber                         node_status_sub_;
    agv_msgs::msg::CANopenNodeStatus             node_status_;
    std::map<std::string, std::future<bool>>        controller_init_futures_;
    ros::ServiceServer                      controller_enable_srv_;
};

}